import visual
import math

class Arm :

    def __init__(self,
                 baseheight = 7.9,
                 baseradius = 4.925,
                 armlength = 9.525,
                 forearmlength = 9.525,
                 wristlength = 1,
                 wristrotatelength = 1,
                 griplength = 5) :

        self.table = visual.box( pos=(0,20,-0.25),
                                 size=(0.5,50,80,),
                                 axis=(0,0,-0.5) )

        self.basef = visual.frame( pos = (0,0,0), axis=(0,0,1) )
        self.basef.command = 1500
        self.basef.raxis = (0,0,-1)
        
        self.base = visual.cylinder( pos=(0,0,0),
                                     axis=(baseheight, 0, 0),
                                     radius = baseradius,
                                     
                                    color=visual.color.red,
                                    frame = self.basef,
                                    )

        self.shoulderf = visual.frame( pos=(baseheight,0,0),
                                       axis=(1,0,0),
                                       frame = self.basef )
        self.shoulderf.command = 1500
        self.shoulderf.raxis = (0,0,-1)

        self.arm = visual.cylinder(frame = self.shoulderf,
                                   axis = (armlength, 0,0, ),
                                   radius = 1,
                                   color=visual.color.blue )

        self.elbowf = visual.frame( pos=(armlength, 0,0),
                                    frame = self.shoulderf )
        self.elbowf.command = 834 # 60 degrees
        self.elbowf.raxis = (0,0,1)
        
        self.forearm = visual.cylinder( frame = self.elbowf,
                                        axis = (forearmlength, 0,0 ),
                                        radius = 1,
                                        color=visual.color.green )

        self.wristf = visual.frame( pos=(forearmlength,0,0),
                                    frame = self.elbowf )
        self.wristf.command = 1500
        self.wristf.raxis = (0,0,1)
        
        self.wrist = visual.cylinder( pos=(0,0,0),
                                      axis=(wristlength, 0,0),
                                      radius = 1,
                                      frame = self.wristf,
                                      color = visual.color.magenta
                                      )
        
        self.wristrotatef = visual.frame( pos=(wristlength, 0,0),
                                          frame = self.wristf )
        self.wristrotatef.command = 1500
        self.wristrotatef.raxis = (1,0,0)

        self.grip = visual.box( pos=( wristrotatelength / 2, 0, 0 ),
                                axis = ( 1,0,0 ),
                                height = 1,
                                width = 2,
                                length = wristrotatelength,
                                frame = self.wristrotatef,
                                color = visual.color.yellow

                                )
        
        self.gripf = visual.frame( pos=( wristrotatelength, 0, 0 ),
                                   frame = self.wristrotatef )
        self.gripf.command = 2500

        self.finger1 = visual.cylinder( pos = ( 0, 0, 1 ),
                                        radius = 0.4,
                                        axis = ( griplength, 0, 0),
                                        frame = self.gripf,
                                        color = visual.color.blue
                                        )

        self.finger2 = visual.cylinder( pos = ( 0, 0, -1 ),
                                        radius = 0.4,
                                        axis = ( griplength, 0, 0),
                                        frame = self.gripf,
                                        color = visual.color.blue
                                        )
        
        
    def rotatecmd( self, frame, command ) :
        if command < 500 :
            command = 500
        elif command > 2500 :
            command = 2500
            
        delta = command - frame.command
        angle = delta / 2000 * math.pi

        frame.rotate( angle = angle, axis = frame.raxis )
        frame.command = command
        
    def cbase( self, command ) :
        self.rotatecmd( self.basef, command )
        
    def cshoulder( self, command ) :
        self.rotatecmd( self.shoulderf, command )
        
    def celbow( self, command ) :
        self.rotatecmd( self.elbowf, command )
        
    def cwrist( self, command ) :
        self.rotatecmd( self.wristf, command )
        
    def cwristrotate( self, command ) :
        self.rotatecmd( self.wristrotatef, command )

    def cgrip( self, command ) :
        if command < 500 :
            command = 500
        elif command > 2500 :
            command = 2500
            
        rel = (command - 500) / 2000

        pos = 0.4 + rel * (1.0 - 0.4)

        self.finger1.z = pos
        self.finger2.z = -pos
        
        self.gripf.command = command

if __name__ == "__main__" :

    visual.display( title = "Lynx Simulator",
                    width = 800,
                    height = 600,
                    forward = (-1, 0, -1),
                    up = (0, 0, 1),
                    autoscale = True
                    )
    a = Arm()

    a.cbase( 1500 )
    a.cshoulder( 1500 )
    a.celbow(1500)
    a.cwrist(1500)
    a.cgrip(1500)

    def mv( c ):
        for i in range(1500, 500, -20 ) :
            visual.rate(20)
            c( i )
        for i in range(500, 1500, 20 ) :
            visual.rate(20)
            c( i )
        
    while True :
        #mv( a.cbase)
        #mv( a.cshoulder)
        #mv( a.celbow)
        #mv( a.cwrist)
        mv( a.cwristrotate)
        #mv( a.cgrip)


    while False :
        for i in range(500, 2500, 20 ) :
            visual.rate(10)
            a.cbase( i )
        for i in range(2500, 500, -20 ) :
            visual.rate(10)
            a.cbase( i )
            
        

